def get_angle_casadi(self, point1, point2, point3): v12 = w.vector3(*point1) - w.vector3(*point2) v13 = w.vector3(*point1) - w.vector3(*point3) v23 = w.vector3(*point2) - w.vector3(*point3) d12 = w.norm(v12) d13 = w.norm(v13) d23 = w.norm(v23) # return w.acos(w.dot(v12, v13.T)[0] / (d12 * d13)) return w.acos((d12**2 + d13**2 - d23**2) / (2 * d12 * d13))
def get_constraint(self): soft_constraints = OrderedDict() weight = self.get_input_float(self.weight) root_T_tip = self.get_fk(self.root, self.tip) goal_point = self.get_goal_point() pointing_axis = self.get_pointing_axis() goal_axis = goal_point - w.position_of(root_T_tip) goal_axis /= w.norm(goal_axis) # FIXME possible /0 current_axis = w.dot(root_T_tip, pointing_axis) diff = goal_axis - current_axis soft_constraints[str(self) + u'x'] = SoftConstraint(lower=diff[0], upper=diff[0], weight=weight, expression=current_axis[0]) soft_constraints[str(self) + u'y'] = SoftConstraint(lower=diff[1], upper=diff[1], weight=weight, expression=current_axis[1]) soft_constraints[str(self) + u'z'] = SoftConstraint(lower=diff[2], upper=diff[2], weight=weight, expression=current_axis[2]) return soft_constraints
def get_constraint(self): """ example: name='CartesianPosition' parameter_value_pair='{ "root": "base_footprint", #required "tip": "r_gripper_tool_frame", #required "goal_position": {"header": {"stamp": {"secs": 0, "nsecs": 0}, "frame_id": "", "seq": 0}, "pose": {"position": {"y": 0.0, "x": 0.0, "z": 0.0}, "orientation": {"y": 0.0, "x": 0.0, "z": 0.0, "w": 0.0} } }', #required "weight": 1, #optional "gain": 3, #optional -- error is multiplied with this value "max_speed": 0.3 #optional -- rad/s or m/s depending on joint; can not go higher than urdf limit }' :return: """ goal_position = w.position_of(self.get_goal_pose()) weight = self.get_input_float(self.weight) gain = self.get_input_float(self.gain) max_speed = self.get_input_float(self.max_speed) t = self.get_input_sampling_period() current_position = w.position_of(self.get_fk(self.root, self.tip)) soft_constraints = OrderedDict() trans_error_vector = goal_position - current_position trans_error = w.norm(trans_error_vector) trans_scale = w.diffable_min_fast(trans_error * gain, max_speed * t) trans_control = w.save_division(trans_error_vector, trans_error) * trans_scale soft_constraints[str(self) + u'x'] = SoftConstraint(lower=trans_control[0], upper=trans_control[0], weight=weight, expression=current_position[0]) soft_constraints[str(self) + u'y'] = SoftConstraint(lower=trans_control[1], upper=trans_control[1], weight=weight, expression=current_position[1]) soft_constraints[str(self) + u'z'] = SoftConstraint(lower=trans_control[2], upper=trans_control[2], weight=weight, expression=current_position[2]) return soft_constraints
def get_constraint(self): goal_position = w.position_of(self.get_goal_pose()) weight = self.get_input_float(self.weight) gain = self.get_input_float(self.gain) max_speed = self.get_input_float(self.max_speed) t = self.get_input_sampling_period() current_position = w.position_of(self.get_fk(self.root, self.tip)) soft_constraints = OrderedDict() trans_error_vector = goal_position - current_position trans_error = w.norm(trans_error_vector) trans_scale = w.diffable_min_fast(trans_error * gain, max_speed * t) trans_control = w.save_division(trans_error_vector, trans_error) * trans_scale soft_constraints[str(self) + u'y'] = SoftConstraint(lower=trans_control[1], upper=trans_control[1], weight=weight, expression=current_position[1]) return soft_constraints