Skip to content

Commit

Permalink
Update JointControllerActuator
Browse files Browse the repository at this point in the history 
Fix ros interface, now reading current value and speed
Fix MoveZLift taking 4 secs
  • Loading branch information
@LeroyR
LeroyR committed Mar 1, 2025
1 parent 1837916 commit 1950005
Show file tree
Hide file tree
Showing 3 changed files with 77 additions and 33 deletions.
91 changes: 66 additions & 25 deletions ...pter_ros/src/main/java/de/unibi/citec/clf/bonsai/ros/actuators/RosJointPositionControl.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,39 +15,50 @@ import java.util.concurrent.Future
import control_msgs.FollowJointTrajectoryActionFeedback
import control_msgs.FollowJointTrajectoryActionGoal
import control_msgs.FollowJointTrajectoryActionResult
import control_msgs.JointTrajectoryControllerState
import de.unibi.citec.clf.bonsai.util.BoundSynchronizedQueue
import de.unibi.citec.clf.btl.units.TimeUnit
import de.unibi.citec.clf.btl.units.UnitConverter
import org.ros.message.MessageListener
import org.ros.node.topic.Subscriber
import trajectory_msgs.JointTrajectoryPoint
import kotlin.math.abs

/**
*
* @author lruegeme
*/
class RosJointPositionControl(private val nodeName: GraphName) : RosNode(), JointControllerActuator {

class RosJointPositionControl(private val nodeName: GraphName) : RosNode(), JointControllerActuator, MessageListener<JointTrajectoryControllerState> {

private val queue: BoundSynchronizedQueue<JointTrajectoryControllerState> = BoundSynchronizedQueue(2);
private var state: Subscriber<JointTrajectoryControllerState>? = null
private var ac: ActionClient<FollowJointTrajectoryActionGoal, FollowJointTrajectoryActionFeedback, FollowJointTrajectoryActionResult>? = null
private lateinit var control_topic: String
private lateinit var joint: String
private lateinit var controllerTopic: String
private lateinit var jointName: String
private val logger = org.apache.log4j.Logger.getLogger(javaClass)
private var joint = -1

init {
initialized = false
}

override fun onStart(connectedNode: ConnectedNode) {
state = connectedNode.newSubscriber("${this.controllerTopic}/state", JointTrajectoryControllerState._TYPE)
state?.addMessageListener(this)
ac = ActionClient(
connectedNode,
this.control_topic,
"$controllerTopic/follow_joint_trajectory",
FollowJointTrajectoryActionGoal._TYPE,
FollowJointTrajectoryActionFeedback._TYPE,
FollowJointTrajectoryActionResult._TYPE
)


if (ac?.waitForActionServerToStart(Duration(20.0)) == true) {
logger.info("RosJointController connected to $control_topic")
logger.info("connected to $controllerTopic/follow_joint_trajectory")
initialized = true
} else {
logger.error("RosJointController not started to $control_topic")
logger.error("not started for $controllerTopic")
}

}
Expand All @@ -62,29 +73,50 @@ class RosJointPositionControl(private val nodeName: GraphName) : RosNode(), Join

@Throws(ConfigurationException::class)
override fun configure(ioc: IObjectConfigurator) {
this.control_topic = ioc.requestValue("topic")
this.joint = ioc.requestValue("joint")
this.controllerTopic = ioc.requestValue("controller")
this.jointName = ioc.requestValue("joint")
}


@Throws(IOException::class)
override fun moveTo(pose: Float, duration: Float?): Future<Boolean> {
//TODO clamp to min/max


override fun moveTo(pose: Float, speed: Double): Future<Boolean> {
ac?.let { client ->
val goal = client.newGoalMessage()

var traj = MsgTypeFactory.getInstance().newMessage<JointTrajectoryPoint>(JointTrajectoryPoint._TYPE)
val current = getPosition() ?: throw IOException("cant get current joint pose")

val delta = abs(pose - current)
val duration = (delta / speed) * 1000

val traj = MsgTypeFactory.getInstance().newMessage<JointTrajectoryPoint>(JointTrajectoryPoint._TYPE)
traj.positions = doubleArrayOf(pose.toDouble())

//val delta = abs(getPosition() - pose)
//val duration_sec = delta / (speed?:1f)
val duration_sec = 1f / (duration?:0.25f)
traj.timeFromStart = Duration.fromMillis(duration.toLong())

goal.goal.trajectory.jointNames.add(jointName)
goal.goal.trajectory.points.add(traj)

goal.goal.goalTimeTolerance = Duration.fromMillis(2000)

val sendGoal = client.sendGoal(goal)

logger.info("Ros Joint Position Control: $goal")

return sendGoal.toBooleanFuture()
}

throw IOException()
}

override fun moveTo(pose: Float, duration: Long, unit: TimeUnit): Future<Boolean> {
ac?.let { client ->
val goal = client.newGoalMessage()

traj.timeFromStart = Duration.fromMillis((duration_sec * 1000).toLong())
val traj = MsgTypeFactory.getInstance().newMessage<JointTrajectoryPoint>(JointTrajectoryPoint._TYPE)
traj.positions = doubleArrayOf(pose.toDouble())
traj.timeFromStart = Duration.fromMillis(UnitConverter.convert(duration,unit,TimeUnit.MILLISECONDS))

goal.goal.trajectory.jointNames.add(joint)
goal.goal.trajectory.jointNames.add(jointName)
goal.goal.trajectory.points.add(traj)

goal.goal.goalTimeTolerance = Duration.fromMillis(2000)
Expand All @@ -99,18 +131,27 @@ class RosJointPositionControl(private val nodeName: GraphName) : RosNode(), Join
throw IOException()
}

override fun getMax(): Float {

override fun getMax(): Double? {
throw NotImplementedError()
}

override fun getMin(): Float {
override fun getMin(): Double? {
throw NotImplementedError()
}

override fun getPosition(): Float {
//TODO
//return 0.0f
throw NotImplementedError()
override fun getPosition(): Double? {
val current = queue.front()
return if (joint == -1) null
else current.actual.positions[joint]
}

override fun onNewMessage(p0: JointTrajectoryControllerState?) {
queue.push(p0)
if (joint == -1) {
joint = p0?.jointNames?.indexOf(jointName) ?: -1
logger.warn("found joint: '$jointName' at position $joint")
}
}

}
11 changes: 7 additions & 4 deletions ...i_interfaces/src/main/java/de/unibi/citec/clf/bonsai/actuators/JointControllerActuator.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,8 @@ package de.unibi.citec.clf.bonsai.actuators


import de.unibi.citec.clf.bonsai.core.`object`.Actuator
import de.unibi.citec.clf.btl.data.common.Timestamp
import de.unibi.citec.clf.btl.units.TimeUnit
import java.io.IOException
import java.util.concurrent.Future

Expand All @@ -17,10 +19,11 @@ interface JointControllerActuator : Actuator {
*/

@Throws(IOException::class)
fun moveTo(pose: Float, duration: Float?): Future<Boolean>
fun moveTo(pose: Float, duration: Long, unit: TimeUnit = TimeUnit.MILLISECONDS): Future<Boolean>
fun moveTo(pose: Float, speed: Double): Future<Boolean>

fun getMax(): Float
fun getMin(): Float
fun getPosition(): Float
fun getMax(): Double?
fun getMin(): Double?
fun getPosition(): Double?

}
8 changes: 4 additions & 4 deletions bonsai_skills/src/main/java/de/unibi/citec/clf/bonsai/skills/body/MoveZlift.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ class MoveZlift: AbstractSkill() {
private var tokenError: ExitToken? = null

private var pos = 0f
private var move_duration = 4000
private var speed = 0.07
private var timeout: Long = 7000

private var b: Future<Boolean>? = null
Expand All @@ -68,7 +68,7 @@ class MoveZlift: AbstractSkill() {
tokenSuccess = configurator.requestExitToken(ExitStatus.SUCCESS())
tokenError = configurator.requestExitToken(ExitStatus.ERROR())

move_duration = configurator.requestOptionalInt(KEY_MOVE_DURATION, move_duration)
speed = configurator.requestOptionalDouble(KEY_SPEED, speed)
timeout = configurator.requestOptionalInt(KEY_TIMEOUT, timeout.toInt()).toLong()

if (configurator.hasConfigurationKey(KEY_POSITION)) {
Expand All @@ -83,7 +83,7 @@ class MoveZlift: AbstractSkill() {
pos
}

b = jointcontroller!!.moveTo(pos, 1000.0f / move_duration)
b = jointcontroller!!.moveTo(pos, speed)

if (timeout > 0) {
timeout += Time.currentTimeMillis()
Expand All @@ -107,7 +107,7 @@ class MoveZlift: AbstractSkill() {

companion object {
private const val KEY_POSITION = "#_POSITION"
private const val KEY_MOVE_DURATION = "#_MOVE_DURATION"
private const val KEY_SPEED = "#_SPEED"
private const val KEY_TIMEOUT = "#_TIMEOUT"
}
}

0 comments on commit 1950005

Please sign in to comment.