def add_cmd(self):
"""
Adds another command to the goal sequence. Any set goal calls will be added the this new goal.
This is used, if you want Giskard to plan multiple goals in succession.
"""
move_cmd = MoveCmd()
self.cmd_seq.append(move_cmd)
def test_empty_joint_goal2(self, zero_pose):
"""
:type zero_pose: Donbot
"""
# zero_pose.allow_self_collision()
goal = MoveActionGoal()
move_cmd = MoveCmd()
joint_goal1 = JointConstraint()
joint_goal1.type = JointConstraint.JOINT
joint_goal1.goal_state.name = [
u'ur5_shoulder_pan_joint', u'ur5_shoulder_lift_joint',
u'ur5_elbow_joint', u'ur5_wrist_1_joint', u'ur5_wrist_2_joint',
u'ur5_wrist_3_joint'
]
joint_goal1.goal_state.position = [
-0.15841275850404912, -2.2956998983966272, 2.240689277648926,
-2.608211342488424, -2.7356796900378626, -2.5249870459186
]
joint_goal2 = JointConstraint()
joint_goal2.type = JointConstraint.JOINT
move_cmd.joint_constraints.append(joint_goal1)
move_cmd.joint_constraints.append(joint_goal2)
goal.goal.cmd_seq.append(move_cmd)
goal.goal.type = goal.goal.PLAN_AND_EXECUTE
zero_pose.send_and_check_goal(goal=goal)
#!/usr/bin/env python
import rospy
from actionlib import SimpleActionClient
from giskard_msgs.msg import MoveAction, MoveGoal, MoveCmd, Controller
if __name__ == '__main__':
rospy.init_node('init_chain')
client = SimpleActionClient('qp_controller/command', MoveAction)
client.wait_for_server()
roots = rospy.get_param('~roots')
tips = rospy.get_param('~tips')
typess = rospy.get_param('~types')
goal = MoveGoal()
move_cmd = MoveCmd()
if not (len(roots) == len(tips) and len(tips) == len(typess)):
raise Exception('number of roots, tips and types not equal')
for root, tip, types in zip(roots, tips, typess):
for type in types:
controller = Controller()
controller.root_link = root
controller.tip_link = tip
controller.type = type
controller.weight = 0
controller.goal_pose.pose.orientation.w = 1
controller.goal_pose.header.frame_id = tip
move_cmd.controllers.append(controller)
goal.type = MoveGoal.PLAN_ONLY
goal.cmd_seq.append(move_cmd)
print('sending goal')
client.send_goal_and_wait(goal)
def add_cmd(self, max_trajectory_length=20):
move_cmd = MoveCmd()
# move_cmd.max_trajectory_length = max_trajectory_length
self.cmd_seq.append(move_cmd)
def execute_joint_goal():
# Creates the SimpleActionClient, passing the type of the action
# (MoveAction) to the constructor.
client = actionlib.SimpleActionClient("/giskardpy/command", MoveAction)
# Waits until the action server has started up and started
# listening for goals.
print('waiting for giskard')
client.wait_for_server()
print('connected to giskard')
# Creates a goal to send to the action server.
action_goal = MoveGoal()
action_goal.type = MoveGoal.PLAN_AND_EXECUTE
goal = MoveCmd()
joint_goal = JointConstraint()
joint_goal.type = JointConstraint.JOINT
# this can be any subset of the robots joints
# joint_goal.goal_state is a normal sensor_msgs/JointState
joint_goal.goal_state.name = [
"odom_x_joint", "odom_y_joint", "odom_z_joint",
"fr_caster_rotation_joint", "fr_caster_l_wheel_joint",
"fr_caster_r_wheel_joint", "bl_caster_rotation_joint",
"bl_caster_l_wheel_joint", "bl_caster_r_wheel_joint",
"br_caster_rotation_joint", "br_caster_l_wheel_joint",
"br_caster_r_wheel_joint", "torso_lift_joint",
"torso_lift_motor_screw_joint", "head_pan_joint", "head_tilt_joint",
"laser_tilt_mount_joint", "r_shoulder_pan_joint",
"r_shoulder_lift_joint", "r_upper_arm_roll_joint",
"r_forearm_roll_joint", "r_elbow_flex_joint", "r_wrist_flex_joint",
"r_wrist_roll_joint", "r_gripper_motor_slider_joint",
"r_gripper_motor_screw_joint", "r_gripper_l_finger_joint",
"r_gripper_r_finger_joint", "r_gripper_l_finger_tip_joint",
"r_gripper_r_finger_tip_joint", "r_gripper_joint",
"l_shoulder_pan_joint", "l_shoulder_lift_joint",
"l_upper_arm_roll_joint", "l_forearm_roll_joint", "l_elbow_flex_joint",
"l_wrist_flex_joint", "l_wrist_roll_joint",
"l_gripper_motor_slider_joint", "l_gripper_motor_screw_joint",
"l_gripper_l_finger_joint", "l_gripper_r_finger_joint",
"l_gripper_l_finger_tip_joint", "l_gripper_r_finger_tip_joint",
"l_gripper_joint", "fl_caster_rotation_joint",
"fl_caster_l_wheel_joint", "fl_caster_r_wheel_joint"
]
joint_goal.goal_state.position = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.1687062500000004, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-1.1400000000000001, -1.0499999999999994, 0.0, 0.0, 0.0, 0.54, 0.54,
0.54, 0.54, 2.220446049250313e-16, 0.0, 0.0, 0.0, 0.0,
-1.1399999999999988, -1.0499999999999994, 0.0, 0.0, 0.0, 0.54, 0.54,
0.54, 0.54, 2.220446049250313e-16, 0.0, 0.0, 0.0
]
#[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.220446049250313e-16,
#0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -2.220446049250313e-16,
#-2.220446049250313e-16, 0.0, 0.0, 0.0, 0.54, 0.54, 0.54, 0.54,
#2.220446049250313e-16, 0.0, 0.0, 0.0, 0.0, -2.220446049250313e-16,
#-2.220446049250313e-16, 0.0, 0.0, 0.0, 0.54, 0.54, 0.54, 0.54,
#2.220446049250313e-16, 0.0, 0.0, 0.0]
goal.joint_constraints.append(joint_goal)
action_goal.cmd_seq.append(goal)
# Sends the goal to the action server.
client.send_goal(action_goal)
# Waits for the server to finish performing the action.
client.wait_for_result()
result = client.get_result() # type: MoveResult
if result.error_code == MoveResult.SUCCESS:
print('giskard returned success')
else:
print('something went wrong')