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)
task_wrist = Task('wrist_task')
task_wrist.controlGain.value = 1.
task_wrist.add(feature_wrist.name)
# Create operational point for the end effector
model.createOpPoint("ee", "ee_fixed_joint")
solver = SOT('solver')
solver.setSize(dimension)
solver.push(task_wrist.name)
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)
task_wrist = Task ('wrist_task')
task_wrist.controlGain.value = 1.
task_wrist.add (feature_wrist.name)
# Create operational point for the end effector
model.createOpPoint ("ee", "ee_fixed_joint")
solver = SOT ('solver')
solver.setSize (dimension)
solver.push (task_wrist.name)
plug (solver.control, device.control)
