device.after.addSignal(signal)
I4 = (
(1., 0, 0, 0),
(0, 1., 0, 0),
(0, 0, 1., 0),
(0, 0, 0, 1.),
)
model = RosRobotModel('ur5_dynamic')
device = RobotSimu('ur5_device')
rospy.init_node('fake')
model.loadUrdf(
"file:///local/ngiftsun/devel/ros/catkin_ws/src/ur_description/urdf/ur5_robot.urdf"
)
dimension = model.getDimension()
device.resize(dimension)
plug(device.state, model.position)
# Set velocity and acceleration to 0
model.velocity.value = dimension * (0., )
model.acceleration.value = dimension * (0., )
# Create taks for the base
model.createOpPoint("base", "waist")
# Create task for the wrist
model.createOpPoint("wrist", "wrist_3_joint")
signal = '{0}.{1}'.format(entityName, signalName)
filename = '{0}-{1}'.format(entityName, signalName)
trace.add(signal, filename)
if autoRecompute:
device.after.addSignal(signal)
I4 = ((1.,0,0,0),
(0,1.,0,0),
(0,0,1.,0),
(0,0,0,1.),)
model = RosRobotModel ('ur5_dynamic')
device = RobotSimu ('ur5_device')
rospy.init_node('fake')
model.loadUrdf ("file:///local/ngiftsun/devel/ros/catkin_ws/src/ur_description/urdf/ur5_robot.urdf")
dimension = model.getDimension ()
device.resize (dimension)
plug (device.state, model.position)
# Set velocity and acceleration to 0
model.velocity.value = dimension * (0.,)
model.acceleration.value = dimension * (0.,)
# Create taks for the base
model.createOpPoint ("base", "waist")
# Create task for the wrist
model.createOpPoint ("wrist", "wrist_3_joint")
feature_wrist = FeaturePosition ('position_wrist', model.wrist, model.Jwrist, I4)
